/****************************************************************************
 *
 *   Copyright (C) 2024 MicoAir Development Team. All rights reserved.
 *
 ****************************************************************************/

/**
 * @file test_intrusive_sorted_list.c
 *
 * Unit tests for intrusive sorted list.
 */

#include "px4_platform_common/containers/intrusive_sorted_list.h"
#include <stddef.h>
#include <string.h>

/* External debug printf */
extern void debug_printf(const char *format, ...);

/* Test result tracking */
static int test_count = 0;
static int test_passed = 0;
static int test_failed = 0;

#define TEST_ASSERT(condition, message) \
	do { \
		test_count++; \
		if (condition) { \
			test_passed++; \
		} else { \
			test_failed++; \
			debug_printf("  ❌ FAIL: %s\r\n", message); \
		} \
	} while (0)

/* Test item structure */
typedef struct {
	intrusive_sorted_list_node_t node;
	int value;
} test_item_t;

/* Comparison function: sort by value (ascending) */
static bool test_item_compare(const void *a, const void *b)
{
	const test_item_t *item_a = (const test_item_t *)a;
	const test_item_t *item_b = (const test_item_t *)b;
	return item_a->value <= item_b->value;
}

/* Test 1: Initialization and empty check */
static void test_init_and_empty(void)
{
	debug_printf("Test 1: Initialization and empty check\r\n");

	intrusive_sorted_list_t list;
	intrusive_sorted_list_init(&list, offsetof(test_item_t, node), test_item_compare);

	TEST_ASSERT(intrusive_sorted_list_empty(&list), "List should be empty initially");
	TEST_ASSERT(intrusive_sorted_list_size(&list) == 0, "Size should be 0 initially");
	TEST_ASSERT(intrusive_sorted_list_head(&list) == NULL, "Head should be NULL initially");
}

/* Test 2: Add single item */
static void test_add_single(void)
{
	debug_printf("Test 2: Add single item\r\n");

	intrusive_sorted_list_t list;
	intrusive_sorted_list_init(&list, offsetof(test_item_t, node), test_item_compare);

	test_item_t item1 = {.value = 42};
	intrusive_sorted_list_add(&list, &item1);

	TEST_ASSERT(!intrusive_sorted_list_empty(&list), "List should not be empty");
	TEST_ASSERT(intrusive_sorted_list_size(&list) == 1, "Size should be 1");
	TEST_ASSERT(intrusive_sorted_list_head(&list) == &item1, "Head should be item1");
}

/* Test 3: Add multiple items in order */
static void test_add_multiple_ordered(void)
{
	debug_printf("Test 3: Add multiple items in order\r\n");

	intrusive_sorted_list_t list;
	intrusive_sorted_list_init(&list, offsetof(test_item_t, node), test_item_compare);

	test_item_t items[5];
	for (int i = 0; i < 5; i++) {
		items[i].value = i * 10;  // 0, 10, 20, 30, 40
		intrusive_sorted_list_add(&list, &items[i]);
	}

	TEST_ASSERT(intrusive_sorted_list_size(&list) == 5, "Size should be 5");

	/* Verify order */
	test_item_t *current = (test_item_t *)intrusive_sorted_list_head(&list);
	for (int i = 0; i < 5; i++) {
		TEST_ASSERT(current != NULL, "Item should not be NULL");
		TEST_ASSERT(current->value == i * 10, "Item value should match");
		current = (test_item_t *)intrusive_sorted_list_next(&list, current);
	}
	TEST_ASSERT(current == NULL, "Should reach end of list");
}

/* Test 4: Add multiple items in reverse order */
static void test_add_multiple_reverse(void)
{
	debug_printf("Test 4: Add multiple items in reverse order\r\n");

	intrusive_sorted_list_t list;
	intrusive_sorted_list_init(&list, offsetof(test_item_t, node), test_item_compare);

	test_item_t items[5];
	for (int i = 4; i >= 0; i--) {
		items[i].value = i * 10;  // 40, 30, 20, 10, 0
		intrusive_sorted_list_add(&list, &items[i]);
	}

	TEST_ASSERT(intrusive_sorted_list_size(&list) == 5, "Size should be 5");

	/* Verify order (should be sorted: 0, 10, 20, 30, 40) */
	test_item_t *current = (test_item_t *)intrusive_sorted_list_head(&list);
	for (int i = 0; i < 5; i++) {
		TEST_ASSERT(current != NULL, "Item should not be NULL");
		TEST_ASSERT(current->value == i * 10, "Item value should be sorted");
		current = (test_item_t *)intrusive_sorted_list_next(&list, current);
	}
	TEST_ASSERT(current == NULL, "Should reach end of list");
}

/* Test 5: Add multiple items in random order */
static void test_add_multiple_random(void)
{
	debug_printf("Test 5: Add multiple items in random order\r\n");

	intrusive_sorted_list_t list;
	intrusive_sorted_list_init(&list, offsetof(test_item_t, node), test_item_compare);

	test_item_t items[5];
	int values[] = {30, 10, 40, 0, 20};  // Random order
	for (int i = 0; i < 5; i++) {
		items[i].value = values[i];
		intrusive_sorted_list_add(&list, &items[i]);
	}

	TEST_ASSERT(intrusive_sorted_list_size(&list) == 5, "Size should be 5");

	/* Verify order (should be sorted: 0, 10, 20, 30, 40) */
	test_item_t *current = (test_item_t *)intrusive_sorted_list_head(&list);
	for (int i = 0; i < 5; i++) {
		TEST_ASSERT(current != NULL, "Item should not be NULL");
		TEST_ASSERT(current->value == i * 10, "Item value should be sorted");
		current = (test_item_t *)intrusive_sorted_list_next(&list, current);
	}
}

/* Test 6: Remove head */
static void test_remove_head(void)
{
	debug_printf("Test 6: Remove head\r\n");

	intrusive_sorted_list_t list;
	intrusive_sorted_list_init(&list, offsetof(test_item_t, node), test_item_compare);

	test_item_t items[3];
	for (int i = 0; i < 3; i++) {
		items[i].value = i * 10;
		intrusive_sorted_list_add(&list, &items[i]);
	}

	TEST_ASSERT(intrusive_sorted_list_size(&list) == 3, "Size should be 3");

	/* Remove head (value 0) */
	bool removed = intrusive_sorted_list_remove(&list, &items[0]);
	TEST_ASSERT(removed, "Remove should succeed");
	TEST_ASSERT(intrusive_sorted_list_size(&list) == 2, "Size should be 2");

	/* Verify new head is items[1] (value 10) */
	test_item_t *head = (test_item_t *)intrusive_sorted_list_head(&list);
	TEST_ASSERT(head == &items[1], "New head should be items[1]");
	TEST_ASSERT(head->value == 10, "New head value should be 10");
}

/* Test 7: Remove middle */
static void test_remove_middle(void)
{
	debug_printf("Test 7: Remove middle\r\n");

	intrusive_sorted_list_t list;
	intrusive_sorted_list_init(&list, offsetof(test_item_t, node), test_item_compare);

	test_item_t items[3];
	for (int i = 0; i < 3; i++) {
		items[i].value = i * 10;
		intrusive_sorted_list_add(&list, &items[i]);
	}

	/* Remove middle (value 10) */
	bool removed = intrusive_sorted_list_remove(&list, &items[1]);
	TEST_ASSERT(removed, "Remove should succeed");
	TEST_ASSERT(intrusive_sorted_list_size(&list) == 2, "Size should be 2");

	/* Verify order: 0, 20 */
	test_item_t *current = (test_item_t *)intrusive_sorted_list_head(&list);
	TEST_ASSERT(current->value == 0, "First item should be 0");
	current = (test_item_t *)intrusive_sorted_list_next(&list, current);
	TEST_ASSERT(current->value == 20, "Second item should be 20");
}

/* Test 8: Remove tail */
static void test_remove_tail(void)
{
	debug_printf("Test 8: Remove tail\r\n");

	intrusive_sorted_list_t list;
	intrusive_sorted_list_init(&list, offsetof(test_item_t, node), test_item_compare);

	test_item_t items[3];
	for (int i = 0; i < 3; i++) {
		items[i].value = i * 10;
		intrusive_sorted_list_add(&list, &items[i]);
	}

	/* Remove tail (value 20) */
	bool removed = intrusive_sorted_list_remove(&list, &items[2]);
	TEST_ASSERT(removed, "Remove should succeed");
	TEST_ASSERT(intrusive_sorted_list_size(&list) == 2, "Size should be 2");

	/* Verify order: 0, 10 */
	test_item_t *current = (test_item_t *)intrusive_sorted_list_head(&list);
	TEST_ASSERT(current->value == 0, "First item should be 0");
	current = (test_item_t *)intrusive_sorted_list_next(&list, current);
	TEST_ASSERT(current->value == 10, "Second item should be 10");
	current = (test_item_t *)intrusive_sorted_list_next(&list, current);
	TEST_ASSERT(current == NULL, "Should reach end");
}

/* Test 9: Remove non-existent item */
static void test_remove_nonexistent(void)
{
	debug_printf("Test 9: Remove non-existent item\r\n");

	intrusive_sorted_list_t list;
	intrusive_sorted_list_init(&list, offsetof(test_item_t, node), test_item_compare);

	test_item_t items[2];
	items[0].value = 10;
	items[1].value = 20;
	intrusive_sorted_list_add(&list, &items[0]);

	/* Try to remove item that's not in the list */
	bool removed = intrusive_sorted_list_remove(&list, &items[1]);
	TEST_ASSERT(!removed, "Remove should fail");
	TEST_ASSERT(intrusive_sorted_list_size(&list) == 1, "Size should still be 1");
}

/* Test 10: Clear list */
static void test_clear(void)
{
	debug_printf("Test 10: Clear list\r\n");

	intrusive_sorted_list_t list;
	intrusive_sorted_list_init(&list, offsetof(test_item_t, node), test_item_compare);

	test_item_t items[5];
	for (int i = 0; i < 5; i++) {
		items[i].value = i * 10;
		intrusive_sorted_list_add(&list, &items[i]);
	}

	TEST_ASSERT(intrusive_sorted_list_size(&list) == 5, "Size should be 5");

	intrusive_sorted_list_clear(&list);

	TEST_ASSERT(intrusive_sorted_list_empty(&list), "List should be empty");
	TEST_ASSERT(intrusive_sorted_list_size(&list) == 0, "Size should be 0");
	TEST_ASSERT(intrusive_sorted_list_head(&list) == NULL, "Head should be NULL");
}

/* Test 11: Duplicate values */
static void test_duplicate_values(void)
{
	debug_printf("Test 11: Duplicate values\r\n");

	intrusive_sorted_list_t list;
	intrusive_sorted_list_init(&list, offsetof(test_item_t, node), test_item_compare);

	test_item_t items[5];
	items[0].value = 10;
	items[1].value = 20;
	items[2].value = 20;  // Duplicate
	items[3].value = 20;  // Duplicate
	items[4].value = 30;

	for (int i = 0; i < 5; i++) {
		intrusive_sorted_list_add(&list, &items[i]);
	}

	TEST_ASSERT(intrusive_sorted_list_size(&list) == 5, "Size should be 5");

	/* Verify all items are in list */
	test_item_t *current = (test_item_t *)intrusive_sorted_list_head(&list);
	TEST_ASSERT(current->value == 10, "First should be 10");
	current = (test_item_t *)intrusive_sorted_list_next(&list, current);
	TEST_ASSERT(current->value == 20, "Second should be 20");
	current = (test_item_t *)intrusive_sorted_list_next(&list, current);
	TEST_ASSERT(current->value == 20, "Third should be 20");
	current = (test_item_t *)intrusive_sorted_list_next(&list, current);
	TEST_ASSERT(current->value == 20, "Fourth should be 20");
	current = (test_item_t *)intrusive_sorted_list_next(&list, current);
	TEST_ASSERT(current->value == 30, "Fifth should be 30");
}

/* Test 12: Large list */
static void test_large_list(void)
{
	debug_printf("Test 12: Large list (100 items)\r\n");

	intrusive_sorted_list_t list;
	intrusive_sorted_list_init(&list, offsetof(test_item_t, node), test_item_compare);

	test_item_t items[100];

	/* Add in reverse order */
	for (int i = 99; i >= 0; i--) {
		items[i].value = i;
		intrusive_sorted_list_add(&list, &items[i]);
	}

	TEST_ASSERT(intrusive_sorted_list_size(&list) == 100, "Size should be 100");

	/* Verify sorted order */
	test_item_t *current = (test_item_t *)intrusive_sorted_list_head(&list);
	for (int i = 0; i < 100; i++) {
		TEST_ASSERT(current != NULL, "Item should not be NULL");
		TEST_ASSERT(current->value == i, "Item value should match");
		current = (test_item_t *)intrusive_sorted_list_next(&list, current);
	}
	TEST_ASSERT(current == NULL, "Should reach end");
}

/* Test 13: Remove all items one by one */
static void test_remove_all(void)
{
	debug_printf("Test 13: Remove all items one by one\r\n");

	intrusive_sorted_list_t list;
	intrusive_sorted_list_init(&list, offsetof(test_item_t, node), test_item_compare);

	test_item_t items[10];
	for (int i = 0; i < 10; i++) {
		items[i].value = i * 10;
		intrusive_sorted_list_add(&list, &items[i]);
	}

	TEST_ASSERT(intrusive_sorted_list_size(&list) == 10, "Size should be 10");

	/* Remove all items */
	for (int i = 0; i < 10; i++) {
		bool removed = intrusive_sorted_list_remove(&list, &items[i]);
		TEST_ASSERT(removed, "Remove should succeed");
		TEST_ASSERT(intrusive_sorted_list_size(&list) == 10 - i - 1, "Size should decrease");
	}

	TEST_ASSERT(intrusive_sorted_list_empty(&list), "List should be empty");
}

/* Test 14: Re-insert after remove */
static void test_reinsert(void)
{
	debug_printf("Test 14: Re-insert after remove\r\n");

	intrusive_sorted_list_t list;
	intrusive_sorted_list_init(&list, offsetof(test_item_t, node), test_item_compare);

	test_item_t items[5];
	for (int i = 0; i < 5; i++) {
		items[i].value = i * 10;
		intrusive_sorted_list_add(&list, &items[i]);
	}

	/* Remove middle item */
	intrusive_sorted_list_remove(&list, &items[2]);
	TEST_ASSERT(intrusive_sorted_list_size(&list) == 4, "Size should be 4");

	/* Re-insert it */
	intrusive_sorted_list_add(&list, &items[2]);
	TEST_ASSERT(intrusive_sorted_list_size(&list) == 5, "Size should be 5");

	/* Verify order is still correct */
	test_item_t *current = (test_item_t *)intrusive_sorted_list_head(&list);
	for (int i = 0; i < 5; i++) {
		TEST_ASSERT(current->value == i * 10, "Item value should match");
		current = (test_item_t *)intrusive_sorted_list_next(&list, current);
	}
}

/* Test 15: Edge cases */
static void test_edge_cases(void)
{
	debug_printf("Test 15: Edge cases\r\n");

	intrusive_sorted_list_t list;
	intrusive_sorted_list_init(&list, offsetof(test_item_t, node), test_item_compare);

	/* Remove from empty list */
	test_item_t item;
	item.value = 42;
	bool removed = intrusive_sorted_list_remove(&list, &item);
	TEST_ASSERT(!removed, "Remove from empty list should fail");

	/* Add NULL */
	intrusive_sorted_list_add(&list, NULL);
	TEST_ASSERT(intrusive_sorted_list_empty(&list), "Adding NULL should not change list");

	/* Remove NULL */
	removed = intrusive_sorted_list_remove(&list, NULL);
	TEST_ASSERT(!removed, "Remove NULL should fail");

	/* Clear empty list */
	intrusive_sorted_list_clear(&list);
	TEST_ASSERT(intrusive_sorted_list_empty(&list), "Clear empty list should be safe");
}

/* Main test runner */
void intrusive_sorted_list_run_tests(void)
{
	debug_printf("\r\n");
	debug_printf("========================================\r\n");
	debug_printf("  Intrusive Sorted List Tests\r\n");
	debug_printf("========================================\r\n");

	test_count = 0;
	test_passed = 0;
	test_failed = 0;

	test_init_and_empty();
	test_add_single();
	test_add_multiple_ordered();
	test_add_multiple_reverse();
	test_add_multiple_random();
	test_remove_head();
	test_remove_middle();
	test_remove_tail();
	test_remove_nonexistent();
	test_clear();
	test_duplicate_values();
	test_large_list();
	test_remove_all();
	test_reinsert();
	test_edge_cases();

	debug_printf("========================================\r\n");
	debug_printf("  Test Summary\r\n");
	debug_printf("========================================\r\n");
	debug_printf("Total tests: %d\r\n", test_count);
	debug_printf("Passed: %d\r\n", test_passed);
	debug_printf("Failed: %d\r\n", test_failed);
	debug_printf("\r\n");

	if (test_failed == 0) {
		debug_printf("✅ All intrusive_sorted_list tests PASSED!\r\n");
	} else {
		debug_printf("❌ Some intrusive_sorted_list tests FAILED!\r\n");
	}

	debug_printf("========================================\r\n");
	debug_printf("\r\n");
}

